﻿#include "mainwindow.h"
#include "ui_mainwindow.h"
#include "conview.h"
#include "forceview.h"
#include <QHBoxLayout>
#include "jointview.h"
#include "posview.h"
#include "urparser.h"
#include <QDebug>


MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
{
    ui->setupUi(this);
//    ui->dockWidget_forceinput->setWindowTitle(tr("Force Input"));
//        ui->dockWidget_joint->setWindowTitle("Joint value");
//    ui->dockWidget_position->setWindowTitle("Rob position");
//    ui->dockWidget_control_plat->setWindowTitle("Control");
    initforms();
    hap_ = new HapticDV();
    zerofactor_={0,0,0,0,0,0};
    setWindowTitle(QString::fromLocal8Bit("机械臂实时力控操作器"));

}

MainWindow::~MainWindow()
{
    delete ui;
}

void MainWindow::initforms()
{
    QHBoxLayout* conlayout=new QHBoxLayout();
    conlayout->addWidget(ConView::Instance());
    ui->frame_cotrol->setLayout(conlayout);
    QHBoxLayout* forclayout= new QHBoxLayout();
    forclayout->addWidget(forceview::Instance());
    ui->groupBox_force->setLayout(forclayout);
    QHBoxLayout* jointlayout= new QHBoxLayout(ui->groupBox_Joint);
    jointlayout->addWidget(JointView::Instance());
    QHBoxLayout* poslayout= new QHBoxLayout(ui->groupBox_Pos);
    poslayout->addWidget(PosView::Instance());
    connect(ConView::Instance(),SIGNAL(sig_connected()),this,SLOT(slot_connectestablished()));
    connect(ConView::Instance(),SIGNAL(sig_disconnected()),this,SLOT(slot_disconnected()));
    refreshtimer=new QTimer(this);
    connect(refreshtimer,SIGNAL(timeout()),this,SLOT(slot_datarefresh()));
    ui->horizontalSlider->setValue(80);
    ui->horizontalSlider_2->setValue(100);
    ui->label_force_factor->setText("0.80");
    ui->label_speed_factor->setText("1.00");
    ui->groupBox_factor->setEnabled(false);

}

void MainWindow::calibrationForce()
{
    forcecount=0;
}

void MainWindow::updateHandPosition(std::vector<double> pos)
{
    if(pos.size()<6)
        return;
    ui->doubleSpinBox_hpx->setValue(pos.at(0));
    ui->doubleSpinBox_hpy->setValue(pos.at(1));
    ui->doubleSpinBox_hpz->setValue(pos.at(2));
    ui->doubleSpinBox_hprx->setValue(pos.at(3));
    ui->doubleSpinBox_hpry->setValue(pos.at(4));
    ui->doubleSpinBox_hprz->setValue(pos.at(5));
}

void MainWindow::updateHandSpeed(std::vector<double> pos)
{
    if(pos.size()<6)
        return;
    ui->doubleSpinBox_hspx->setValue(pos.at(0));
    ui->doubleSpinBox_hspy->setValue(pos.at(1));
    ui->doubleSpinBox_hspz->setValue(pos.at(2));
    ui->doubleSpinBox_hsprx->setValue(pos.at(3));
    ui->doubleSpinBox_hspry->setValue(pos.at(4));
    ui->doubleSpinBox_hsprz->setValue(pos.at(5));
}

void MainWindow::slot_connectestablished()
{
    qDebug()<<"slot_connectestablished :";
    refreshtimer->start(100);
    ui->groupBox_factor->setEnabled(true);
    //URParser::Instance()->recive();
}

void MainWindow::slot_disconnected()
{
    if(refreshtimer->isActive())
        refreshtimer->stop();
    ui->groupBox_factor->setEnabled(false);
}

void MainWindow::slot_datarefresh()
{
    std::vector<double> q = URParser::Instance()->getActualQ();
    std::vector<double> p= URParser::Instance()->getActualTcpPos();
    std::vector<double> f=URParser::Instance()->getActuralTcpForce();
    PosView::Instance()->setValues(p);
    JointView::Instance()->setValues(q);
    forceview::Instance()->setValues(f);
    std::vector<double> hp= hap_->GetHapticData();
    updateHandPosition(hp);
    //qDebug()<<"position ==  "<<QVector<double>::fromStdVector(hp);
    std::vector<double> sp= hap_->GetHapticSpeed();
    updateHandSpeed(sp);
    //qDebug()<<"speed ==  "<<QVector<double>::fromStdVector(sp);
    int ios=URParser::Instance()->getIO_input(0);
    if(forcecount<10)
    {
        for(int i=0;i<f.size();++i)
        {
            zerofactor_[i]+=f.at(i);
        }
        forcecount++;
        ui->pushButton_calib->setEnabled(false);
    }
    else if(forcecount==10)
    {
        for(int i=0;i<zerofactor_.size();++i)
        {
            zerofactor_[i] /= forcecount;
        }
        hap_->setZeroForce(zerofactor_);
        //forceview::Instance()->setCalibrationdata(zerofactor_);
        forcecount++;
        ui->pushButton_calib->setEnabled(true);
    }
    if( ios != IO_input)
    {
        IO_input=ios;
        std::cout<<"io changed "<<IO_input<<std::endl;
    }

    if(IO_input>0)
    {
        ui->label_status->setStyleSheet("background-color: rgb(0, 255, 0);");
        if(factor_<1.0)
            factor_ += 0.05;
        hap_->setforcefactor(factor_);
        if(!hap_->setHapticForce(f))
            std::cout<<"force lost"<<std::endl;
        std::vector<double> nsp={0,0,0,0,0,0};
        nsp[0]=sp.at(0);
        nsp[1]=sp.at(1);
        nsp[2]=sp.at(2);
        if(ui->checkBox_onlypos->isChecked())
            URParser::Instance()->speedl(nsp);
        else
            URParser::Instance()->speedl(sp);
    }
    else
    {
        factor_=0;
        URParser::Instance()->stopspeedL();
        ui->label_status->setStyleSheet("background-color: rgb(255, 0,0);");
    }
}


//void MainWindow::on_pushButton_test_clicked()
//{
//    QString comd=ui->plainTextEdit->toPlainText();
//    if(URParser::Instance()->isconnected())
//    {
//        URParser::Instance()->movej(comd);
//    }
//}

//void MainWindow::on_pushButton_SpeedL_pressed()
//{
//    QString comd=ui->plainTextEdit->toPlainText();
//    if(URParser::Instance()->isconnected())
//    {
//        URParser::Instance()->speedl(comd);
//    }
//}

//void MainWindow::on_pushButton_SpeedL_released()
//{
//    if(URParser::Instance()->isconnected())
//    {
//        URParser::Instance()->stopspeedL();
//    }
//}

void MainWindow::on_pushButton_calib_clicked()
{
    calibrationForce();
}

void MainWindow::on_horizontalSlider_valueChanged(int value)
{
    double vl= value/100.0;
    ui->label_force_factor->setText(QString::number(vl,'f',2));
    hap_->setMaxfactor(vl);
}

void MainWindow::on_horizontalSlider_2_valueChanged(int value)
{
    double vl= value/100.0;
    ui->label_speed_factor->setText(QString::number(vl,'f',2));
    URParser::Instance()->setSpeedFactor(vl);
}
